#include <string.h>

#include <WConstants.h>
#include <pins_arduino.h>

#include "OLED.h"

byte OLED::getResponse() {
  byte incomingByte = OLED_NAK;
  boolean timeOutOccurred = false;
  unsigned long startedMillis = millis();
  while ((serialPort.available() == 0) && !timeOutOccurred) {
    if (millis() >= startedMillis + 1000) {
      timeOutOccurred = true;
      break;
    }
  }
  if (!timeOutOccurred) {
    incomingByte = serialPort.read();
  }
  return incomingByte;
}

void OLED::write(byte value) {
  serialPort.print(value, BYTE);
}


OLED::OLED(int rxPin, int txPin, int resetPin) :
#if defined(__AVR_ATmega32U4__) // Teensy 2.0
  // Nothing to do for Teensy
#else
  // Init the NewSoftSerial object
  serialPort(rxPin, txPin),
#endif
  txPin(txPin),
  resetPin(resetPin) { }

byte OLED::init() {
  pinMode(txPin, OUTPUT);
  pinMode(resetPin, OUTPUT);

  digitalWrite(resetPin, HIGH);
  delay(1);
  digitalWrite(resetPin, LOW);
  delay(1);
  digitalWrite(txPin, HIGH);
  digitalWrite(resetPin, HIGH);
  serialPort.begin(OLED_BAUD_RATE);
  serialPort.flush();
  delay(OLED_INIT_DELAY);
  write(GSGC_AUTOBAUD);

  byte incomingByte = getResponse();
  return incomingByte;
}

unsigned int OLED::get16bitColourFromRGB(byte red, byte green, byte blue) {
  byte r = red >> 3;  // red: 0-31
  byte g = green >> 2;  // green: 0-63
  byte b = blue >> 3; // blue: 0-31

  unsigned int colourIn16bit = (r << 11) + (g << 5) + b;
  return colourIn16bit;
}

byte OLED::clearScreen() {
  write(GSGC_CLS);
  byte incomingByte = getResponse();
  return incomingByte;
}


byte OLED::displayControlFunctions(byte mode, byte value) {
  write(GSGC_DISPCONT);
  write(mode);
  write(value);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::displayOn() {
  return displayControlFunctions(GSGC_DISPLAY_ON_OFF_COMMAND, 0x01);
}

byte OLED::displayOff() {
  return displayControlFunctions(GSGC_DISPLAY_ON_OFF_COMMAND, 0x00);
}

byte OLED::contrastAdjust(byte value) {
  return displayControlFunctions(GSGC_SET_CONTRAST, value);
}

byte OLED::displayPowerUp() {
  return displayControlFunctions(GSGC_POWER_UP_POWER_DOWN_COMMAND, 0x01);
}

byte OLED::displayShutdown() {
  return displayControlFunctions(GSGC_POWER_UP_POWER_DOWN_COMMAND, 0x00);
}

byte OLED::sound(byte note, unsigned int duration) {
  write(GSGC_SOUND);
  write(note);
  write(duration >> 8);
  write(duration & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawCircle(byte x, byte y, byte radius, unsigned int colour)  {
  write(GSGC_CIRCLE);
  write(x);
  write(y);
  write(radius);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawRectangle(byte x1, byte y1, byte x2, byte y2, unsigned int colour) {
  write(GSGC_RECTANGLE);
  write(x1);
  write(y1);
  write(x2);
  write(y2);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawLine(byte x1, byte y1, byte x2, byte y2, unsigned int colour) {
  write(GSGC_LINE);
  write(x1);
  write(y1);
  write(x2);
  write(y2);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::setFont(byte fontSet) {
  write(GSGC_SETFONT);
  write(fontSet);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::setTransparentText() {
  write(GSGC_SETOPAQUE);
  write(0x00);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::setOpaqueText() {
  write(GSGC_SETOPAQUE);
  write(0x01);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawStringAsText(byte column, byte row, byte fontSet, char *myText, unsigned int colour) {
  write(GSGC_STRINGTXT);
  write(column);
  write(row);
  write(fontSet);
  write(colour >> 8);
  write(colour & 0xFF);
  for (int i = 0; i < strlen(myText); i++) {
    write(myText[i]);
  }
  write(GSGC_TERMINATOR);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::setPenSize(byte state) {
  write(GSGC_SETPEN);
  write(state);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawCharacterAsText(byte column, byte row, char myCharacter, unsigned int colour) {
  write(GSGC_CHARTXT);
  write(myCharacter);
  write(column);
  write(row);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawPixel(byte x, byte y, unsigned int colour) {
  write(GSGC_PIXEL);
  write(x);
  write(y);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

unsigned int OLED::readPixel(byte x, byte y) {
  write(GSGC_RDPIXEL);
  write(x);
  write(y);
  byte incomingByte = getResponse();
  unsigned int colour = incomingByte << 8;
  incomingByte = getResponse();
  colour += incomingByte;
  return colour;
}

byte OLED::drawTriangle(byte x1, byte y1, byte x2, byte y2, byte x3, byte y3, unsigned int colour) {
  write(GSGC_TRIANGLE);
  write(x1);
  write(y1);
  write(x2);
  write(y2);
  write(x3);
  write(y3);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::setBackgroundColour(unsigned int colour) {
  write(GSGC_BACKGND);
  write(colour >> 8);
  write(colour & 0xFF);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawTextButton(byte state, byte x, byte y, unsigned int buttonColour, byte font, unsigned int stringColour, byte width, byte height, char *buttonText) {
  write(GSGC_BUTTONTXT);
  write(state);
  write(x);
  write(y);
  write(buttonColour >> 8);
  write(buttonColour & 0xFF);
  write(font);
  write(stringColour >> 8);
  write(stringColour & 0xFF);
  write(width);
  write(height);
  for (int i = 0; i < strlen(buttonText); i++) {
    write(buttonText[i]);
  }
  write(GSGC_TERMINATOR);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawStringAsGraphics(byte x, byte y, byte fontSet, char *myText, unsigned int colour, byte width, byte height) {
  write(GSGC_STRINGGFX);
  write(x);
  write(y);
  write(fontSet);
  write(colour >> 8);
  write(colour & 0xFF);
  write(width);
  write(height);
  for (int i = 0 ; i < strlen(myText); i++) {
    write(myText[i]);
  }
  write(GSGC_TERMINATOR);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::drawCharacterAsGraphics(byte x, byte y, char myCharacter, unsigned int colour, byte width, byte height) {
  write(GSGC_CHARGFX);
  write(myCharacter);
  write(x);
  write(y);
  write(colour >> 8);
  write(colour & 0xFF);
  write(width);
  write(height);
  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::displayImageIconFromMemoryCard(byte x, byte y, byte width, byte height, byte colourMode, unsigned long sectorAddress) {
  write(GSGC_EXTCMD);
  write(GSGC_MCIMAGE);
  write(x);
  write(y);
  write(width);
  write(height);
  write(colourMode);
  write((byte)(sectorAddress >> 16));
  write((byte)(sectorAddress >> 8));
  write((byte)(sectorAddress & 0xFF));

  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::displayObjectFromMemoryCard(byte x, byte y, byte width, byte height, byte colourMode, unsigned long sectorAddress) {
  write(GSGC_EXTCMD);
  write(GSGC_MCOBJ);
  write((byte)(sectorAddress >> 24));
  write((byte)(sectorAddress >> 16));
  write((byte)(sectorAddress >> 8));
  write((byte)(sectorAddress & 0xFF));

  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::runScriptProgramFromMemoryCard(unsigned long address) {
  write(GSGC_EXTCMD);
  write(GSGC_MCRUN);
  write((byte)(address >> 24));
  write((byte)(address >> 16));
  write((byte)(address >> 8));
  write((byte)(address & 0xFF));

  byte incomingByte = getResponse();
  return incomingByte;
}

byte OLED::displayVideoAnimationClipFromMemoryCard(byte x, byte y, byte width, byte height, byte colourMode, byte delay, unsigned int frames, unsigned long sectorAddress) {
  write(GSGC_EXTCMD);
  write(GSGC_MCVIDEO);
  write(x);
  write(y);
  write(width);
  write(height);
  write(colourMode);
  write(delay);
  write(frames >> 8);
  write(frames & 0xFF);
  write((byte)(sectorAddress >> 16));
  write((byte)(sectorAddress >> 8));
  write((byte)(sectorAddress & 0xFF));

  byte incomingByte = getResponse();
  return incomingByte;
}
